#include <stdio.h>
#include <stdlib.h>
#include <assert.h>

#include "test/test.h"
#include "greatest/greatest.h"
#if (USE_SMP==1u)
#if (configUSE_CORE_AFFINITY==1u)
SUITE(suit_core_affinity);

k_stack_t test1_task_stack_00[TEST_TASK_STACK_SIZE_00];
k_stack_t test1_task_stack_01[TEST_TASK_STACK_SIZE_01];
k_stack_t test1_task_stack_02[TEST_TASK_STACK_SIZE_02];

k_task_t test1_task_00;
k_task_t test1_task_01;
k_task_t test1_task_02;
static int cnt1=1;
static int cnt2=1;
static void test_task_entry(void *arg)
{
    k_tick_t delay;

    // if arg == K_NULL, delay 2000 ticks(default)
    delay = arg ? *((k_tick_t *)arg) : (k_tick_t)100u;

    while (K_TRUE) {
        tos_task_delay(delay);
        cnt1++;
    }
}

static void test_task_entry01(void *arg)
{
    while (K_TRUE) {
        cnt2++;
    }
}

TEST test_tos_set_get_affinity(void)
{
    k_err_t err;
    err = tos_task_create(&test1_task_00, "test_task", test_task_entry,
                            K_NULL, TEST_TASK_PRIO_00,
                            test1_task_stack_00, sizeof(test1_task_stack_00),
                            0);
    ASSERT_EQ(err, K_ERR_NONE);

    BaseType_t CoreAffinityMask=(1<<1);
    BaseType_t  result;
    tos_TaskCoreAffinitySet( &test1_task_00,  CoreAffinityMask );
    result=vTaskCoreAffinityGet(&test1_task_00);

    ASSERT_EQ(result, CoreAffinityMask);
   

    CoreAffinityMask=(1<<0)|(1<<1);
    tos_TaskCoreAffinitySet( &test1_task_00,  CoreAffinityMask );
    result=vTaskCoreAffinityGet(&test1_task_00);
    ASSERT_EQ(result, CoreAffinityMask);


    CoreAffinityMask=(1<<4)|(1<<1);
    tos_TaskCoreAffinitySet( &test1_task_00,  CoreAffinityMask );
    result=vTaskCoreAffinityGet(&test1_task_00);
    ASSERT_EQ(result, (1<<1));

    err = tos_task_destroy(&test1_task_00);
    ASSERT_EQ(err, K_ERR_NONE);

    PASS();
}
TEST test_tos_affinity_run(void){
    k_err_t err;
    err = tos_task_create(&test1_task_01, "test_task", test_task_entry01,
                            K_NULL, TEST_TASK_PRIO_00,
                            test1_task_stack_01, sizeof(test1_task_stack_01),
                            0);
    ASSERT_EQ(err, K_ERR_NONE);

    err = tos_task_create(&test1_task_00, "test_task", test_task_entry,
                            K_NULL, TEST_TASK_PRIO_00,
                            test1_task_stack_00, sizeof(test1_task_stack_00),
                            0);
    ASSERT_EQ(err, K_ERR_NONE);
    ASSERT_EQ(cnt1, 1);
    BaseType_t coreid=port_GET_CORE_ID();
    BaseType_t CoreAffinityMask=(1<<coreid);
    tos_TaskCoreAffinitySet( &test1_task_01,  CoreAffinityMask );
    int cur=cnt2;
    while(cnt1<30){
        ;
    }
    ASSERT_EQ(cnt2,cur);
    tos_task_delay(1000);
    ASSERT(cnt2>cur);
    err = tos_task_destroy(&test1_task_00);
    ASSERT_EQ(err, K_ERR_NONE);
    err = tos_task_destroy(&test1_task_01);
    ASSERT_EQ(err, K_ERR_NONE);
    PASS();

}
SUITE(suit_core_affinity)
{
    RUN_TEST(test_tos_set_get_affinity);
    RUN_TEST(test_tos_affinity_run);
}
#endif
#endif
